#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

#include <core/balance_task.hpp>

BalanceTask::BalanceTask(std::shared_ptr<Context> context)
    : Task("FollowPath", context)
{
    cs_pub = context->GetNode()
                 ->create_publisher<general_interface::msg::ChassisServo>(
                     "control_signal", 3);
}

bool BalanceTask::OnInit()
{
    context->ActiveNode(Context::StateController);
    context->DeactiveNode(Context::NodeName::PathController);
    context->DeactiveNode(Context::NodeName::LocalPlaner);
    return true;
}

int BalanceTask::OnStep(double &precent)
{
    general_interface::msg::ChassisServo cs;
    cs.speed=0;
    cs.gyroz=0;
    cs.speed_gain=1;
    cs_pub->publish(cs);
    precent = 0;
    return 1;
}

bool BalanceTask::OnCancle()
{
    return true;
}